use statrs::distribution::{Continuous, ContinuousCDF, Normal};
use statrs::statistics::Distribution;
pub struct GridMap {
    pub minx: f64,
    pub maxx: f64,
    pub miny: f64,
    pub maxy: f64,
    pub resolution: f64,
    pub std: f64,
}

impl GridMap {
    pub fn width(&self) -> (i64, i64) {
        let x_wid = (self.maxx - self.minx) / self.resolution;
        let y_wid = (self.maxy - self.miny) / self.resolution;
        (x_wid as i64, y_wid as i64)
    }
    pub fn init_gmap(&self) -> Vec<Vec<f64>> {
        let mut gmap = Vec::new();
        let w = self.width();
        for ix in 0..w.0 {
            let mut row = Vec::new();
            for iy in 0..w.1 {
                row.push(0.);
            }
            gmap.push(row);
        }
        gmap
    }
    pub fn gaussina_grid(&self, detect: &Vec<(f64, f64)>) {
        let mut gmap = self.init_gmap();
        let w = self.width();
        for ix in 0..w.0 {
            for iy in 0..w.1 {
                let x = (ix as f64) * self.resolution + self.minx;
                let y = (iy as f64) * self.resolution + self.miny;
                let dis = detect
                    .iter()
                    .map(|v| ((x - v.0).powf(2.) + (y - v.1).powf(2.)).sqrt())
                    .min_by(|a, b| a.total_cmp(b))
                    .unwrap();

                let norm = Normal::new(0., self.std).unwrap();
                gmap[ix as usize][iy as usize] = 1. - norm.cdf(dis);
            }
        }
    }
}

#[test]
fn test_gaussian_map() {
    let map = GridMap {
        minx: -3.,
        miny: -9.,
        maxx: 8.,
        maxy: 4.,
        resolution: 0.5,
        std: 5.,
    };
}
